
#include <dispatch_system/local_costmap.h>

namespace moying
{
namespace navigation
{

LocalCostMap::LocalCostMap(){ 
    
}

bool LocalCostMap::init(MoyingMapPtr moying_map){
    if(moying_map == nullptr)
        return false;
    moying_map_ = moying_map;
    return true;   
}

bool LocalCostMap::buildLocalCostMap(VehiclePtr vehicle){

    if(moying_map_ == nullptr || vehicle == nullptr)
        return false;

    double center_x,center_y;
    center_x = vehicle->getPose().x;
    center_y = vehicle->getPose().y;
    float cost_map_width    = 6;
    float cost_map_height   = 6;
    float resolution    = moying_map_->getCostMap()->getResolution();
    int cell_width    = cost_map_width / resolution;
    int cell_height   = cost_map_height / resolution;
    //初始化局部代价地图
    if(local_cost_map_.empty()){
        local_cost_map_.resize(cell_width+1);
        for(auto& item : local_cost_map_){
            item.resize(cell_height+1,0);
        }
    }else{
        for(auto& item1:local_cost_map_){
            for(auto& item2:item1){
                item2 = 0;
            }
        }
    }

    //计算地图cell边界
    int mx,my;
    if( !moying_map_->getCostMap()->worldToMap(center_x,center_y,mx,my) )
        return false;
    int minx,miny,maxx,maxy;
    minx = mx - cell_width/2;
    miny = my - cell_height/2;
    minx = minx < 0 ? 0 : minx;
    miny = miny < 0 ? 0 : miny;
    maxx = minx + cell_width;
    maxy = miny + cell_height;

    MapDimension d = moying_map_->getDimensions();
    int cell_x = d.x;
    int cell_y = d.y;
    if(maxx >= cell_x){
        maxx = cell_x-1;
        minx = maxx-cell_width;
    }
    if(maxy >= cell_y){
        maxy = cell_y-1;
        miny = maxy-cell_height;
    }
    min_cell_x_ = minx;
    min_cell_y_ = miny;
    max_cell_x_ = maxx;
    max_cell_y_ = maxy;

    LocalObstacleInfo obs_info;
    if(!moying_map_->getLocalObstacle(vehicle->getID(),obs_info))
        return false;
    
    
    //膨胀局部障碍物
    int inflation_num = 1;
    int i,j,k,l;
    int max_cost = (2*inflation_num+1)*2;

    for(auto obs_cell : obs_info.occupancy_cells){
        int i = obs_cell.first;
        int j = obs_cell.second;

        int x_edge_down = std::max( 0 , i - inflation_num);
        int x_edge_up = std::min(cell_x-1, i + inflation_num);
        int y_edge_down = std::max(0,j - inflation_num);
        int y_edge_up = std::min(cell_y-1,j + inflation_num);

        for(k = x_edge_down; k <= x_edge_up; k++)
        {
            for(l = y_edge_down; l <= y_edge_up; l++)
            {
                bool is_obs_cell = false;
                for(auto cell : obs_info.occupancy_cells){
                    if(cell.first == k && cell.second == l){
                        is_obs_cell = true;
                        break;
                    }
                }

                int cost = 0;
                if(is_obs_cell){
                    cost = 255;
                }else{
                    int dis = abs(i-k)+abs(j-l);
                    cost = (max_cost-dis) * 2;
                    cost = cost < 0 ? 0 : cost;
                }
                
                int origin_cost = getCellCost(k,l);
                if(origin_cost < cost){
                    setCellCost(k,l,cost);
                }
                
            }
        }

    }

    // for(int x=0;x<local_cost_map_.size();x++){
    //     for(int y=0;y<local_cost_map_[0].size();y++){
    //         printf("x:%d y:%d cost:%d\n",x+min_cell_x_,y+min_cell_y_,local_cost_map_[x][y]);
    //     }
    // }

}

unsigned char LocalCostMap::getCellCost(int mx,int my){

    if(mx < min_cell_x_ || mx > max_cell_x_ || my < min_cell_y_ || my > max_cell_y_){
        return 0;
    }

    int x = mx - min_cell_x_;
    int y = my - min_cell_y_;
    return local_cost_map_[x][y];
}


bool LocalCostMap::setCellCost(int mx,int my,unsigned char cost){

    if(mx < min_cell_x_ || mx > max_cell_x_ || my < min_cell_y_ || my > max_cell_y_){
        return false;
    }

    int x = mx - min_cell_x_;
    int y = my - min_cell_y_;
    local_cost_map_[x][y] = cost;
    return true;


}




}
}